#include "data_struct/robot/Pose.h"
#include "lib/WrapFuncs.h"

namespace behavior_controller
{
PoseF PoseF::Inversion() const
{
    Orientation orientation = Orientation_.inverse();
    orientation.angle() = Funcs::NormalizeAngle(orientation.angle());
    PoseF pose_tmp = PoseF(Pt2D(-(orientation * GetPoint())), orientation.angle());
    //std::cout<<GetPoint().x()<<" "<< GetPoint().y()<<" "<<Orientation_.angle()<<std::endl;
    //std::cout<<pose_tmp.GetPoint().x()<<" "<< pose_tmp.GetPoint().y()<<" "<<pose_tmp.angle()<<std::endl;
    return pose_tmp;
}

PointF PoseF::Convert(const PointF &pt) const
{
    return PointF(Orientation_ * pt.GetPoint() + GetPoint());
}

PoseF PoseF::Convert(const PoseF &pose) const
{
    return PoseF(
            PointF(Orientation_ * pose.GetPoint() + GetPoint()),
            Funcs::NormalizeAngle((Orientation_ * pose.Orientation_).angle()));
}

//将当前位姿的坐标，转换为 提供Pose 所在坐标系下的坐标
PoseF PoseF::ConvertOnCur(const PoseF &pose) const
{
    return Convert(pose);
}

//将括号内提供的Pose 转换为以 当前位姿坐标为原点坐标系的坐标
PoseF PoseF::ConvertOriginCur(const PoseF &pose) const
{
    return Inversion().Convert(pose);
}

//将当前位姿的坐标，转换为 提供点 所在坐标系下的坐标
PointF PoseF::ConvertOnCur(const PointF &pt) const
{
    return Convert(pt);
}

//将括号内提供的点 转换为以 当前位姿坐标为原点坐标系的坐标
PointF PoseF::ConvertOriginCur(const PointF &pt) const
{
    return Inversion().Convert(pt);
}

//将当前点云转换为机器人坐标系
PointCloud<double> PoseF::ConvertToLocalCloud(PointCloud<double> &point_cloud) const
{
    PointCloud<double> local_point_cloud;
    for (auto pt : point_cloud)
    {
        local_point_cloud.emplace_back(ConvertOriginCur(pt));
    }
    return local_point_cloud;
}

//将当前点云转换为世界坐标系
PointCloud<double> PoseF::ConvertToWorldCloud(PointCloud<double> &point_cloud) const
{
    PointCloud<double> world_point_cloud;
    for (auto pt : point_cloud)
    {
        world_point_cloud.emplace_back(ConvertOnCur(pt));
    }
    return world_point_cloud;
}

} // namespace behavior_controller